// 4 in 2 out instruction queue
module instr_queue #(
  parameter DEPTH = 4,
  parameter WIDTH = 32,
  parameter WAY = 4
) (
  input clk,
  input rst,
  input flush,

  input wvalid,
  output wready,
  input [$clog2(WAY):0] wnum,
  input [WAY*WIDTH-1:0] wdata,

  output [1:0] rvalid,
  input [1:0] rready,
  output [WIDTH*2-1:0] rdata
);

  // total instruction number in the queue
  reg [$clog2(WAY*DEPTH):0] count;

  // write/read begin way number
  reg [$clog2(WAY)-1:0] wway;
  reg [$clog2(WAY)-1:0] rway;

  wire full;
  wire empty;

  // write/read
  wire write = wvalid && wready;
  wire [1:0] read = {
    rready[1] && rvalid[1],
    rready[0] && rvalid[0]
  };

  wire fifo_wvalid [WAY-1:0];
  wire fifo_wready [WAY-1:0];
  wire [WIDTH-1:0] fifo_wdata [WAY-1:0];
  wire [$clog2(WAY)-1:0] fifo_widx [WAY-1:0];
  wire [$clog2(WAY)-1:0] fifo_ridx [WAY-1:0];
  wire [WIDTH-1:0] wdata_vec [WAY-1:0];

  wire fifo_rvalid [WAY-1:0];
  wire fifo_rready [WAY-1:0];
  wire [WIDTH-1:0] fifo_rdata [WAY-1:0];

  always@(posedge clk) begin
    if (rst || flush) begin
      count <= 0;
    end else begin
      if (write && !read) begin
        count <= count + wnum;
      end else if (!write && read) begin
        count <= count - 1;
      end else if (write && read) begin
        count <= count + wnum - 1;
      end
    end
  end

  always@(posedge clk) begin
    if (rst || flush) begin
      wway <= 0;
    end else if (write) begin
      wway <= wway + wnum;
    end
  end

  always@(posedge clk) begin
    if (rst || flush) begin
      rway <= 0;
    end else if (|read) begin
      // read only could be 2'b01 or 2'b11
      if (read == 2'b01) begin
        rway <= rway + 1;
      end else begin
        rway <= rway + 2;
      end
    end
  end


  assign full = count >= (WAY * DEPTH - WAY);
  assign empty = count == 0;

  genvar way, i;
  generate
    for (i = 0; i < WAY; i = i + 1) begin
      assign wdata_vec[i] = wdata[(i + 1)*WIDTH-1:i * WIDTH];
    end
  endgenerate

  generate
    for (way = 0; way < WAY; way = way + 1) begin
      assign fifo_widx[way] = way - wway;
      assign fifo_ridx[way] = way - rway;
      // write into n fifos together (the use of wready)
      assign fifo_wvalid[way] = write && (fifo_widx[way] < wnum);
      assign fifo_wdata[way] = wdata_vec[fifo_widx[way]];
      assign fifo_rready[way] = (fifo_ridx[way] < 2) ? rready[fifo_ridx[way]] : 1'b0;
      syn_fifo #(
        .DEPTH(DEPTH),
        .WIDTH(WIDTH)
      ) mycpu_syn_fifo (
        .clk(clk),
        .rst(rst),
        .flush(flush),

        .wvalid(fifo_wvalid[way]),
        .wready(fifo_wready[way]),
        .wdata(fifo_wdata[way]),

        .rvalid(fifo_rvalid[way]),
        .rready(fifo_rready[way]),
        .rdata(fifo_rdata[way])
      );
    end
  endgenerate

  wire [$clog2(WAY)-1:0] rway_seq = rway + 1;
  assign wready = !full;
  assign rvalid[0] = fifo_rvalid[rway];
  assign rvalid[1] = fifo_rvalid[rway_seq];
  assign rdata = {
    fifo_rdata[rway_seq],
    fifo_rdata[rway]
  };
  
endmodule
